
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sim_aircraft.h
  * @author     baiyang
  * @date       2022-11-28
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "sitl.h"
#include "sitl_input.h"
#include "sim_battery.h"

#include <common/gp_defines.h>
#include <common/location/location.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
enum GroundBehaviour {
    GROUND_BEHAVIOR_NONE = 0,
    GROUND_BEHAVIOR_NO_MOVEMENT,
    GROUND_BEHAVIOR_FWD_ONLY,
    GROUND_BEHAVIOR_TAILSITTER,
};

typedef Vector2f_t Vector2d;
typedef Vector3f_t Vector3d;

typedef struct sim_aircraft* sim_aircraft_t;
typedef const struct sim_aircraft* sim_aircraft_const_t;

typedef const struct sim_aircraft_ops* sim_aircraft_ops_t;

struct sim_aircraft {
    sim_t sitl;

    sim_aircraft_ops_t ops;

    // origin of position vector
    Location origin;

    // home location
    Location home;

    bool home_is_set;
    Location location;

    float ground_level;
    float home_yaw;
    float frame_height;
    matrix3f_t dcm;                        // rotation matrix, APM conventions, from body to earth
    Vector3f_t gyro;                       // rad/s
    Vector3f_t velocity_ef;                // m/s, earth frame
    Vector3f_t wind_ef;                    // m/s, earth frame
    Vector3f_t velocity_air_ef;            // velocity relative to airmass, earth frame
    Vector3f_t velocity_air_bf;            // velocity relative to airmass, body frame
    Vector3d position;                     // meters, NED from origin
    float mass;                            // kg
    float external_payload_mass;           // kg
    Vector3f_t accel_body;                 // m/s/s NED, body frame,default {0.0f, 0.0f, -GRAVITY_MSS}
    float airspeed;                        // m/s, apparent airspeed
    float airspeed_pitot;                  // m/s, apparent airspeed, as seen by fwd pitot tube
    float battery_voltage;                 // default -1.0f
    float battery_current;
    float local_ground_level;              // ground level at local position
    bool lock_step_scheduled;
    uint32_t last_one_hz_ms;

    // battery model
    struct sim_battery battery;

    uint32_t motor_mask;
    float rpm[32];
    uint8_t rcin_chan_count;
    float rcin[12];

    struct {
        // data from simulated laser scanner, if available
        struct vector3f_array points;
        struct float_array ranges;
    } scanner;

    // Rangefinder
    float rangefinder_m[SITL_NUM_RANGEFINDERS];

    // Windvane apparent wind
    struct {
        float speed;
        float direction;
    } wind_vane_apparent;

    // Wind Turbulence simulated Data
    float turbulence_azimuth;
    float turbulence_horizontal_speed;  // m/s
    float turbulence_vertical_speed;    // m/s

    Vector3f_t mag_bf;  // local earth magnetic field vector in Gauss, earth frame

    uint64_t time_now_us;

    float gyro_noise;                       // default: radians(0.1f)
    float accel_noise;                      // default: 0.3f
    float rate_hz;                          // default: 500.0f
    float target_speedup;
    uint64_t frame_time_us;
    uint64_t last_wall_time_us;
    uint32_t last_fps_report_ms;
    int64_t sleep_debt_us;
    uint32_t last_frame_count;
    uint8_t instance;
    const char *autotest_dir;
    const char *frame;
    bool use_time_sync;                     // default: true
    float last_speedup;                     // default: -1.0f
    const char *config_;                    // default: ""

    // allow for AHRS_ORIENTATION
    Param_int8 *ahrs_orientation;
    enum RotationEnum last_imu_rotation;
    Param_float* custom_roll;
    Param_float* custom_pitch;
    Param_float* custom_yaw;

    enum GroundBehaviour ground_behavior;

    bool use_smoothing;

    uint64_t last_time_us;
    uint32_t frame_counter;
    uint32_t last_ground_contact_ms;
    uint32_t  min_sleep_time;    // default: 20000(defined(__CYGWIN__) || defined(__CYGWIN64__)) or 5000

    struct {
        uint64_t last_update_us;
        Vector3f_t accel_body;
        Vector3f_t gyro;
        matrix3f_t rotation_b2e;
        Vector3d position;
        Vector3f_t velocity_ef;
        Location location;
    } smoothing;

    LowPassFiltFloat servo_filter[5];
};

struct sim_aircraft_ops {
    // called directly after constructor:
    void (*set_start_location)(sim_aircraft_t aircraft, const Location *start_loc, const float start_yaw);

    /*  Create and set in/out socket for extenal simulator */
    void (*set_interface_ports)(sim_aircraft_t aircraft, const char* address, const int port_in, const int port_out);

    /*
      step the FDM by one time step
     */
    void (*update)(sim_aircraft_t aircraft, const struct sitl_input *input);

    void (*set_config)(sim_aircraft_t aircraft, const char* config);

    float (*rangefinder_beam_width)(sim_aircraft_const_t aircraft);
    float (*perpendicular_distance_to_rangefinder_surface)(sim_aircraft_const_t aircraft);

    bool (*on_ground)(sim_aircraft_const_t aircraft);
};
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sim_aircraft_ctor(sim_aircraft_t aircraft, sim_aircraft_ops_t ops, const char *frame_str);

/*
  set simulation speedup
 */
void sim_aircraft_set_speedup(sim_aircraft_t aircraft, float speedup);
static inline float sim_aircraft_get_speedup(sim_aircraft_const_t aircraft) { return aircraft->target_speedup; }

uint64_t sim_aircraft_get_wall_time_us();

// return difference in altitude between home position and current loc
float sim_aircraft_ground_height_difference(sim_aircraft_const_t aircraft);

// return current height above ground level (metres)
float sim_aircraft_hagl(sim_aircraft_const_t aircraft);

// update location from position
void sim_aircraft_update_position(sim_aircraft_t aircraft);

// update body magnetic field from position and rotation
void sim_aircraft_update_mag_field_bf(sim_aircraft_t aircraft);

/* advance time by deltat in seconds */
void sim_aircraft_time_advance(sim_aircraft_t aircraft);

/* setup the frame step time */
void sim_aircraft_setup_frame_time(sim_aircraft_t aircraft, float new_rate, float new_speedup);

/* adjust frame_time calculation */
void sim_aircraft_adjust_frame_time(sim_aircraft_t aircraft, float new_rate);

/*
   try to synchronise simulation time with wall clock time, taking
   into account desired speedup
   This tries to take account of possible granularity of
   get_wall_time_us() so it works reasonably well on windows
*/
void sim_aircraft_sync_frame_time(sim_aircraft_t aircraft);

/* add noise based on throttle level (from 0..1) */
void sim_aircraft_add_noise(sim_aircraft_t aircraft, float throttle);

/*
  normal distribution random numbers
  See
  http://en.literateprograms.org/index.php?title=Special:DownloadCode/Box-Muller_transform_%28C%29&oldid=7011
*/
double sim_aircraft_rand_normal(double mean, double stddev);

// fill a sitl_fdm structure from the simulator state
void sim_aircraft_fill_fdm(sim_aircraft_t aircraft, struct sitl_fdm *fdm);

// smooth sensors for kinematic consistancy when we interact with the ground
void sim_aircraft_smooth_sensors(sim_aircraft_t aircraft);
float sim_aircraft_rangefinder_range(sim_aircraft_const_t aircraft);

void sim_aircraft_update_model(sim_aircraft_t aircraft, const struct sitl_input *input);
void sim_aircraft_update_dynamics(sim_aircraft_t aircraft, const Vector3f_t *rot_accel);

// update wind vector
void sim_aircraft_update_wind(sim_aircraft_t aircraft, const struct sitl_input *input);
float sim_aircraft_get_local_updraft(sim_aircraft_t aircraft, const Vector3d *currentPos);

// return a filtered servo input as a value from -1 to 1
// servo is assumed to be 1000 to 2000, trim at 1500
float sim_aircraft_filtered_idx(sim_aircraft_t aircraft, float v, uint8_t idx);

// return a filtered servo input as a value from -1 to 1
// servo is assumed to be 1000 to 2000, trim at 1500
float sim_aircraft_filtered_servo_angle(sim_aircraft_t aircraft, const struct sitl_input *input, uint8_t idx);

// return a filtered servo input as a value from 0 to 1
// servo is assumed to be 1000 to 2000
float sim_aircraft_filtered_servo_range(sim_aircraft_t aircraft, const struct sitl_input *input, uint8_t idx);

// extrapolate sensors by a given delta time in seconds
void sim_aircraft_extrapolate_sensors(sim_aircraft_t aircraft, float delta_time);
void sim_aircraft_update_external_payload(sim_aircraft_t aircraft, const struct sitl_input *input);

void sim_aircraft_add_shove_forces(sim_aircraft_t aircraft, Vector3f_t *rot_accel, Vector3f_t *body_accel);
void sim_aircraft_add_twist_forces(sim_aircraft_t aircraft, Vector3f_t *rot_accel);

// get position relative to home
Vector3d sim_aircraft_get_position_relhome(sim_aircraft_const_t aircraft);

void sim_aircraft_state_publish(sim_aircraft_const_t aircraft);

// set instance number
static inline void sim_aircraft_set_instance(sim_aircraft_t aircraft, uint8_t _instance) {
    aircraft->instance = _instance;
}

//set directory for additional files such as aircraft models
static inline void sim_aircraft_set_autotest_dir(sim_aircraft_t aircraft, const char *_autotest_dir) {
    aircraft->autotest_dir = _autotest_dir;
}

// get frame rate of model in Hz
static inline float sim_aircraft_get_rate_hz(sim_aircraft_const_t aircraft) { return aircraft->rate_hz; }

static inline const Vector3f_t *sim_aircraft_get_gyro(sim_aircraft_const_t aircraft) {
    return &aircraft->gyro;
}

static inline const Vector3f_t *sim_aircraft_get_velocity_ef(sim_aircraft_const_t aircraft) {
    return &aircraft->velocity_ef;
}

static inline const Vector3f_t *sim_aircraft_get_velocity_air_ef(sim_aircraft_const_t aircraft) {
    return &aircraft->velocity_air_ef;
}

static inline const matrix3f_t *sim_aircraft_get_dcm(sim_aircraft_const_t aircraft) {
    return &aircraft->dcm;
}

static inline const Vector3f_t *sim_aircraft_get_mag_field_bf(sim_aircraft_const_t aircraft) {
    return &aircraft->mag_bf;
}

static inline float sim_aircraft_gross_mass(sim_aircraft_const_t aircraft) { return aircraft->mass + aircraft->external_payload_mass; }

static inline const Location* sim_aircraft_get_location(sim_aircraft_const_t aircraft) { return &aircraft->location; }
static inline void sim_aircraft_get_attitude(sim_aircraft_const_t aircraft, Quat_t *attitude) {
        quat_from_rotation_matrix(attitude, &aircraft->dcm);
}

static inline const Location *sim_aircraft_get_home(sim_aircraft_const_t aircraft) { return &aircraft->home; }
static inline float sim_aircraft_get_home_yaw(sim_aircraft_const_t aircraft) { return aircraft->home_yaw; }

static inline float sim_aircraft_get_battery_voltage(sim_aircraft_const_t aircraft) { return aircraft->battery_voltage; }

/// 
void sim_aircraft_SetStartLocation(sim_aircraft_t aircraft, const Location *start_loc, const float start_yaw);
bool sim_aircraft_OnGround(sim_aircraft_const_t aircraft);
float sim_aircraft_PerpendicularDistanceToRangefinderSurface(sim_aircraft_const_t aircraft);

/// ops
static inline void sim_aircraft_set_start_location(sim_aircraft_t aircraft, const Location *start_loc, const float start_yaw) {
    if (aircraft->ops->set_start_location) {
        aircraft->ops->set_start_location(aircraft, start_loc, start_yaw);
        return;
    }

    sim_aircraft_SetStartLocation(aircraft, start_loc, start_yaw);
}

/*  Create and set in/out socket for extenal simulator */
static inline void sim_aircraft_set_interface_ports(sim_aircraft_t aircraft, const char* address, const int port_in, const int port_out) {
    if (aircraft->ops->set_interface_ports) {
        aircraft->ops->set_interface_ports(aircraft, address, port_in, port_out);
    }
}

/*
  step the FDM by one time step
 */
static inline void sim_aircraft_update(sim_aircraft_t aircraft, const struct sitl_input *input) {
    if (aircraft->ops->update) {
        aircraft->ops->update(aircraft, input);
    }

    sim_aircraft_state_publish(aircraft);
}

static inline void sim_aircraft_set_config(sim_aircraft_t aircraft, const char* config) {
    if (aircraft->ops->set_config) {
        aircraft->ops->set_config(aircraft, config);
        return;
    }

    aircraft->config_ = config;
}

static inline float sim_aircraft_rangefinder_beam_width(sim_aircraft_const_t aircraft) {
    if (aircraft->ops->rangefinder_beam_width) {
        return aircraft->ops->rangefinder_beam_width(aircraft);
    }

    return 0.0f;
}

static inline float sim_aircraft_perpendicular_distance_to_rangefinder_surface(sim_aircraft_const_t aircraft) {
    if (aircraft->ops->perpendicular_distance_to_rangefinder_surface) {
        return aircraft->ops->perpendicular_distance_to_rangefinder_surface(aircraft);
    }

    return sim_aircraft_PerpendicularDistanceToRangefinderSurface(aircraft);
}

// return true if we are on the ground
static inline bool sim_aircraft_on_ground(sim_aircraft_const_t aircraft) {
    if (aircraft->ops->on_ground) {
        return aircraft->ops->on_ground(aircraft);
    }

    return sim_aircraft_OnGround(aircraft);
}

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



